Data Association in O(n) for Divide and Conquer SLAM

In this paper we show that all processes associated to the move-sense-update cycle of EKF SLAM can be carried out in time linear in the number of map features. We describe Divide and Conquer SLAM, an EKF SLAM algorithm where the computational complexity per step is reduced from O(n) to O(n) (the total cost of SLAM is reduced from O(n) to O(n)). In addition, the resulting vehicle and map estimates have better consistency properties than standard EKF SLAM in the sense that the computed state covariance more adequately represents the real error in the estimation. Both simulated experiments and the Victoria Park Dataset are used to provide evidence of the advantages of this algorithm.